#pragma once
#include "SC_ESKF/include/eskf.h"
#include "gnss_imu_fusion/utils/geographic_easy_use.hpp"
#include <deque>
struct Obs{
    double stamp;
    SlamCraft::GNSSDataType gnss;
    // . 当有其他观测时，可以一并加入这个数据结构中
    // double yaw;
};
struct ESKFStateCacheItem
{
    SlamCraft::ESKF::State_18 state;
    Eigen::Matrix<double,18,18> P;
    SlamCraft::IMU imu;
    double imu_start_time;
    double imu_end_time;
};
struct Report{
    // . add 操作是否引发了eskf update;
    bool is_obs_update = false;
    // . is_obs_update == true 下面两项有效
    SlamCraft::ESKF::State_18 obs_update_state;
    double obs_update_time;
    // . 预测步是否正常，
    bool is_predict = false;
    // . is_predict ==true 下面两项有效，为predict的结果
    SlamCraft::ESKF::State_18 predict_state;
    double predict_time;
};
class GNSSImuFusion
{
private:
    SlamCraft::ESKF eskf;
    SlamCraft::EZGeographic ez_geo;
    Obs now_obs;
    bool is_inited = false;
    std::deque<SlamCraft::IMU>imu_buffer;
    std::deque<Obs>obs_buffer;
    SlamCraft::IMU last_imu;
    std::deque<ESKFStateCacheItem> eskf_state_cache;

public:
    GNSSImuFusion(/* args */);
    ~GNSSImuFusion();
    void eskf_init();

    void calc_eskf_zhr(const SlamCraft::ESKF::State_18& X,Eigen::MatrixXd & Z,Eigen::MatrixXd & H,Eigen::MatrixXd & R);
    inline bool ready(){
        return is_inited;
    }

    Report add(const SlamCraft::IMU&imu);
    Report add(const Obs&obs);

    SlamCraft::ESKF::State_18 get_state(){
        return eskf.getX();
    }
};

